/**
 * @FilePath     : /src/lgmg_robots/agv_navigation/agv_navigate/agvWorld/GeometrySrc/bezier.cpp
 * @Description  : 实现了贝塞尔曲线的相关功能，包括贝塞尔曲线的创建、计算长度、计算曲率等。 
 *                 提供了多种构造函数以支持不同的输入参数，并实现了从参数t计算曲线上的点、导数和曲率的方法。 
 *                 通过分段计算来估算曲线的总长度和在给定参数区间上的长度。 
 *                 包含了对曲线的切线和二阶导数的计算，支持动态更新当前进度以获取当前点的属性。  
 * @Author       : xiujun.yang
 * @Version      : 1.0.0
 * @LastEditors  : haibo haibo.yang@lgmgim.cn
 * @LastEditTime : 2024-12-19 17:48:41
 * @Copyright (c) 2024 by 临工智能信息科技有限公司, All Rights Reserved. 
**/
#include <math.h> 
#include "bezier.h"
#define CURVE_SPLIT_SEGMENT        300 //200

///////////////////////////////////////////////////////////////////////////////

Bezier::Bezier(Posture& start_posture, Posture& end_posture, float len1, float len2)
{
    // 初始化起点和终点
    key_points_[0] = start_posture.GetPntObject(); // 获取起始姿态的点对象
    key_points_[3] = end_posture.GetPntObject();   // 获取结束姿态的点对象
    
    // 设置长度参数
    len1_ = len1;
    len2_ = len2;
    
    // 根据长度参数计算控制点
    Line ln1(start_posture, len1_);  // 创建从起始姿态出发、长度为len1_的线段
    key_points_[1] = ln1.GetEndPoint(); // 获取线段的终点作为第一个控制点
    
    Line ln2(end_posture, -len2_);   // 创建从结束姿态出发、反向长度为len2_的线段
    key_points_[2] = ln2.GetEndPoint(); // 获取线段的终点作为第二个控制点
	// 创建贝塞尔曲线
    Create();
}

Bezier::Bezier(const Point& start_point, const Point& ctrl_point1, const Point& ctrl_point2, const Point& end_point)
{
    key_points_[0] = start_point;
    key_points_[1] = ctrl_point1;
    key_points_[2] = ctrl_point2;
    key_points_[3] = end_point;
	// 创建贝塞尔曲线
    Create();
}

//
//   Generate Bezier curves.
//
void Bezier::Create()
{
    Line ln1(key_points_[0], key_points_[1]);
    len1_ = ln1.Length();
    Line ln2(key_points_[2], key_points_[3]);
    len2_ = ln2.Length();
    Line ln3(key_points_[1], key_points_[2]);
    len3_ = ln3.Length();

    // 计算贝塞尔多边形长度
    polygon_length_ = len1_ + len2_ + len3_;
    current_radius_ = radius_ = len1_;
    curve_length_ = 0.0f;
    current_t_ = 0.0f;
    last_t_ = 0.0f;
    delta_t_ = 0.0f;

    Line start_line(key_points_[0], key_points_[1]);
    Line end_line(key_points_[2], key_points_[3]);
    start_angle_ = start_line.SlantAngle();
    end_angle_ = end_line.SlantAngle();
    total_length_ = ComputeTotalLength();
}

//
// 根据给定的分段数（segment），计算贝塞尔曲线的总长度
//
float Bezier::ComputeTotalLength(int segment) 
{
    float t = 0.0f;
    float dx1 = 0.0f, dy1 = 0.0f;
    float ds = 0.0f;
    float dt = 0.0f;
    int curve_split_count = 0;

    // 估算曲线长度
    if (segment < 0) {
        curve_split_count = polygon_length_ * CURVE_SPLIT_SEGMENT;
    } else {
        curve_split_count = segment;
    }

    if (curve_split_count <= 0) {
        curve_split_count = 1;
    }

    for (int j = 0; j < CURVE_SEGMENT_COUNT; ++j) {
        segment_data_[j].exist = false;
        segment_data_[j].t = 0.0f;
        segment_data_[j].len = 0.0f;
    }

    dt = 1.0f / curve_split_count;
    for (int i = 0; i < curve_split_count; ++i) {
        t = dt * i;
        FirstDerivative(t);
        dx1 = first_derivative_.dx;
        dy1 = first_derivative_.dy;
        ds += std::sqrt(dx1 * dx1 + dy1 * dy1) * dt;

        if (i == 0) {
            segment_data_[0].exist = true;
            segment_data_[0].t = 0.0f;
            segment_data_[0].len = 0.0f;
        } else if (i == (curve_split_count - 1)) {
            segment_data_[CURVE_SEGMENT_COUNT - 1].exist = true;
            segment_data_[CURVE_SEGMENT_COUNT - 1].t = 1.0f;
            segment_data_[CURVE_SEGMENT_COUNT - 1].len = ds;
        } else {
            int index = static_cast<int>(1.0f * i / curve_split_count * CURVE_SEGMENT_COUNT);
            if (index >= 0 && index < CURVE_SEGMENT_COUNT) {
                if (!segment_data_[index].exist) {
                    segment_data_[index].t = t + dt;
                    segment_data_[index].len = ds;
                    segment_data_[index].exist = true;
                }
            }
        }
    }
    return ds;
}


// normal   t2 > t1
// negative t2 < t1
// 计算贝塞尔曲线在给定参数区间[t1, t2]上的长度	
float Bezier::ComputeCurveLength(float t1, float t2, int segment) 
{
    int curve_split_count = 0;
    float ds = 0.0f;
    float dt = 0.0f;
    bool negative = false;

    if (std::fabs(t2 - t1) < 1e-8f) {
        return 0.0f;
    }
    if (t1 > t2) {
        negative = true;
    }

    if (segment < 0) {
        curve_split_count = polygon_length_ * CURVE_SPLIT_SEGMENT * std::fabs(t2 - t1);
    } else {
        curve_split_count = segment;
    }

    if (curve_split_count <= 0) {
        curve_split_count = 1;
    }

    dt = std::fabs(t2 - t1) / curve_split_count;
    for (int i = 0; i < curve_split_count; ++i) {
        float t = negative ? t2 + dt * i : t1 + dt * i;
        FirstDerivative(t);
        float dx1 = first_derivative_.dx;
        float dy1 = first_derivative_.dy;
        ds += std::sqrt(dx1 * dx1 + dy1 * dy1) * dt;
    }

    return negative ? -ds : ds;
}
//
//	计算贝塞尔曲线在参数t处的相对长度
//
float Bezier::ComputeRelativeLength(float t) 
{
    if (t < 0.0f) t = 0.0f;
    if (t > 1.0f) t = 1.0f;
    return ComputeCurveLength(current_t_, t);
}

//
// In order to calculate the arc length accurately,
// the arc length is calculated from 0 - > T every time.
// In order to speed up the calculation, the whole arc length can be pre stored in sections
// (e.g. divided into 100 sections), from which T1 is taken and T1 - > t arc length is calculated;
//
float Bezier::ComputeAbsoluteLength(float t)
{
    float ds = 0.0f;
    if (t < 0.0f) t = 0.0f;
    if (t > 1.0f) t = 1.0f;

    int index = static_cast<int>(t * CURVE_SEGMENT_COUNT);
    if (index < 0) {
        ds = segment_data_[0].len;
    } else if (index >= CURVE_SEGMENT_COUNT) {
        ds = segment_data_[CURVE_SEGMENT_COUNT - 1].len + ComputeCurveLength(segment_data_[CURVE_SEGMENT_COUNT - 1].t, t);
    } else {
        ds = segment_data_[index].len + ComputeCurveLength(segment_data_[index].t, t);
    }

    return ds;
}

//
// FIXME: maybe it is problem every call m_pt and m_d1 is change maybe is error
// 根据给定的参数t和初始点(x0, y0)，计算贝塞尔曲线上的参考点
float Bezier::ComputeReferencePoint(float t, float x0, float y0) 
{
    if (t < 0.0f) t = 0.0f;
    if (t > 1.0f) t = 1.0f;

    ComputePoint(t);
    FirstDerivative(t);

    float x = trajectory_point_.x_;
    float y = trajectory_point_.y_;
    float dx = first_derivative_.dx;
    float dy = first_derivative_.dy;

    return (y - y0) * dy + (x - x0) * dx;
}

//
// Calculate the trajectory point (x, y)
// 计算贝塞尔曲线在参数t处的轨迹点
Point& Bezier::ComputePoint(float t) 
{
    if (t < 0.0f) t = 0.0f;
    if (t > 1.0f) t = 1.0f;

    float f1 = (1 - t);
    float f2 = f1 * f1;
    float f3 = f2 * f1;

    trajectory_point_.x_ = f3 * key_points_[0].x_ + 3 * t * f2 * key_points_[1].x_ +
                           3 * t * t * f1 * key_points_[2].x_ + t * t * t * key_points_[3].x_;
    trajectory_point_.y_ = f3 * key_points_[0].y_ + 3 * t * f2 * key_points_[1].y_ +
                           3 * t * t * f1 * key_points_[2].y_ + t * t * t * key_points_[3].y_;
    return trajectory_point_;
}


//
// Calculate the first derivative (dx1, dy1)
// 计算贝塞尔曲线的一阶导数
Derivative& Bezier::FirstDerivative(float t) 
{
    if (t < 0.0f) t = 0.0f;
    if (t > 1.0f) t = 1.0f;

    first_derivative_.dx = 3 * (key_points_[1].x_ - key_points_[0].x_) +
                            6 * (key_points_[2].x_ - 2 * key_points_[1].x_ + key_points_[0].x_) * t +
                            3 * (key_points_[3].x_ - 3 * key_points_[2].x_ + 3 * key_points_[1].x_ - key_points_[0].x_) * t * t;
    first_derivative_.dy = 3 * (key_points_[1].y_ - key_points_[0].y_) +
                            6 * (key_points_[2].y_ - 2 * key_points_[1].y_ + key_points_[0].y_) * t +
                            3 * (key_points_[3].y_ - 3 * key_points_[2].y_ + 3 * key_points_[1].y_ - key_points_[0].y_) * t * t;
    return first_derivative_;
}

//
// Calculate the second derivative (dX2, dy2)
// 计算贝塞尔曲线的二阶导数
Derivative& Bezier::SecondDerivative(float t) 
{
    if (t < 0.0f) t = 0.0f;
    if (t > 1.0f) t = 1.0f;

    second_derivative_.dx = 6 * (key_points_[2].x_ - 2 * key_points_[1].x_ + key_points_[0].x_) +
                             6 * (key_points_[3].x_ - 3 * key_points_[2].x_ + 3 * key_points_[1].x_ - key_points_[0].x_) * t;
    second_derivative_.dy = 6 * (key_points_[2].y_ - 2 * key_points_[1].y_ + key_points_[0].y_) +
                             6 * (key_points_[3].y_ - 3 * key_points_[2].y_ + 3 * key_points_[1].y_ - key_points_[0].y_) * t;
    return second_derivative_;
}

//
// Compute the current Curvature
// 计算贝塞尔曲线在参数t处的曲率
float Bezier::ComputeCurvature(float t) 
{
    if (t < 0.0f) t = 0.0f;
    if (t > 1.0f) t = 1.0f;

    FirstDerivative(t);
    SecondDerivative(t);

    float dx1 = first_derivative_.dx;
    float dy1 = first_derivative_.dy;
    float dx2 = second_derivative_.dx;
    float dy2 = second_derivative_.dy;

    float f = dx1 * dx1 + dy1 * dy1;
    f = std::sqrt(f * f * f);
    return (dx1 * dy2 - dx2 * dy1) / f;
}

//
//   Set the current progress to determine the current point.
//	 设置当前进度以确定当前点			
void Bezier::SetCurrentLambda(float t, bool update) 
{
    if (t < 0.0f) t = 0.0f;
    if (t > 1.0f) t = 1.0f;

    if (update) {
        last_t_ = current_t_;
        current_t_ = t;
        delta_t_ = current_t_ - last_t_;
    }

    ComputePoint(t);
    FirstDerivative(t);
    SecondDerivative(t);

    float dx1 = first_derivative_.dx;
    float dy1 = first_derivative_.dy;
    float dx2 = second_derivative_.dx;
    float dy2 = second_derivative_.dy;

    float f = dx1 * dx1 + dy1 * dy1;
    f = std::sqrt(f * f * f);
    curvature_ = (dx1 * dy2 - dx2 * dy1) / f;

    current_radius_ = (std::fabs(curvature_) < 1e-8f) ? 0.0f : 1.0f / curvature_;
    tangent_angle_ = std::atan2(dy1, dx1);

    if (update) {
        curve_length_ = ComputeAbsoluteLength(t);
    }
}

